#include <stdio.h>

/**
 * 简单PID算法实现
 */

// PID控制器结构体
typedef struct
{
    float Kp; // 比例增益
    float Ki; // 积分增益
    float Kd; // 微分增益

    float setpoint;       // 目标值
    float integral;       // 积分项
    float previous_error; // 上一次的误差
} PIDController;

// 初始化PID控制器
void PID_Init(PIDController *pid, float Kp, float Ki, float Kd, float setpoint)
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->setpoint = setpoint;
    pid->integral = 0.0f;
    pid->previous_error = 0.0f;
}

// 计算PID控制器的输出
float PID_Update(PIDController *pid, float current_value, float dt)
{
    float error = pid->setpoint - current_value;
    pid->integral += error * dt;
    float derivative = (error - pid->previous_error) / dt;
    float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
    pid->previous_error = error;
    return output;
}

int main()
{
    PIDController pid;
    float Kp = 1.0f;               // 比例增益
    float Ki = 0.1f;               // 积分增益
    float Kd = 0.01f;              // 微分增益
    float setpoint = 100.0f;       // 目标值
    float current_value = 0.0f;    // 当前值
    float control_variable = 0.0f; // 控制变量
    float dt = 0.1f;               // 时间步长

    // 初始化PID控制器
    PID_Init(&pid, Kp, Ki, Kd, setpoint);

    for (int i = 0; i < 100; i++)
    {
        // 计算PID控制器的输出
        control_variable = PID_Update(&pid, current_value, dt);

        // 更新当前值（这里假设控制变量直接作用于当前值）
        current_value += control_variable * dt;

        // 打印当前状态
        printf("Time: %.1fs, Current Value: %.2f, Control Variable: %.2f\n", i * dt, current_value, control_variable);
    }

    return 0;
}
